% test_navpar2imuout_inc % TODO/LM: 按MATLAB测试框架编写，使用TestCase类

cst = load('earthconstants_wgs84.mat');
cst.CNL = [0 1 0; 1 0 0; 0 0 -1];
cst.uZNN = [0 0 1]';
cst.deg2Rad = pi / 180;
cst.min2Rad = pi / 60 / 180;
cst.sec2Rad = pi / 3600 / 180;

par.dt = 0.005;
par.g = 9.793632;
par.useExtGravity = true;

%% 配置
cfg_trajGen.enableTrajectoryGen = true;
cfg_trajGen.enableCBVdynamic = false;
cfg_trajGen.enableSensErr = false;
cfg_trajGen.enableSensRandErr = false;
cfg_trajGen.enableNavigation = true;
cfg_trajGen.ignore2OdSensErr = true;
cfg_trajGen.IMUOutFilePath = [pwd '\data\IMUOut.txt'];
cfg_trajGen.quantizeIMUOut = false;
if cfg_trajGen.quantizeIMUOut
    cfg_trajGen.IMUOutPrecision = [];
else
    cfg_trajGen.IMUOutPrecision = '%.16e';
end

%% 输入
in_trajGen.genFuncName = 'gentgi_navtest_L_racetrack';
transUniVec = [0 1 0]';
rotVec = zeros(3, 0);
transTimeVec = [10 50 10];
in_trajGen.genFuncInArg = {transUniVec, rotVec, transTimeVec};
% in_trajGen.genFuncInArg = { 600 };

%% 参量
par_trajGen.CBVPeriod = 20;
par_trajGen.samplePeriod = par.dt;
par_trajGen.g = par.g; % 重力加速度，m/s^2

% 加速度计常值误差
par_trajGen.IMU.acc.PS0B = eye(3);
par_trajGen.IMU.acc.dfBiasS = zeros(3, 1);
par_trajGen.IMU.acc.KScal0 = ones(3, 1);
par_trajGen.IMU.acc.dKScalP = zeros(3, 2);
par_trajGen.IMU.acc.dKScalN = zeros(3, 2);
par_trajGen.IMU.acc.dPSS0 = zeros(3);
par_trajGen.IMU.acc.dfQuantS = zeros(3, 1);
par_trajGen.IMU.acc.lB = zeros(3);
par_trajGen.IMU.acc.KAniso = zeros(3, 1);
par_trajGen.IMU.acc.CBA = NaN(3, 3, 3);
for i = 1:3
    par_trajGen.IMU.acc.CBA(:, :, i) = eye(3);
end

% 加速度计随机误差
par_trajGen.IMU.acc.noise.R = 0 * ones(3, 1) * (par_trajGen.g/1e6) / 3600; % 转换前单位μg/h，转换后单位m/s^3
par_trajGen.IMU.acc.noise.K = 0 * ones(3, 1) * (par_trajGen.g/1e6) / 60; % 转换前单位μg/sqrt(h)，转换后单位m/s^(5/2)
par_trajGen.IMU.acc.noise.N = 0 * ones(3, 1) * (par_trajGen.g/1e6) * 60; % 转换前单位μg*sqrt(h)，转换后单位m/s^(3/2)
par_trajGen.IMU.acc.noise.Q = realmin * ones(3, 1) / 3600; % 转换前单位m/h，转换后单位m/s

% 陀螺常值误差
par_trajGen.IMU.gyro.PS0B = eye(3);
par_trajGen.IMU.gyro.domegaIBBiasS = zeros(3, 1);
par_trajGen.IMU.gyro.KScal0 = ones(3, 1);
par_trajGen.IMU.gyro.dKScalP = zeros(3, 2);
par_trajGen.IMU.gyro.dKScalN = zeros(3, 2);
par_trajGen.IMU.gyro.dPSS0 = zeros(3);
par_trajGen.IMU.gyro.domegaIBQuantS = zeros(3, 1);
par_trajGen.IMU.gyro.DGSens = zeros(3);

% 陀螺随机误差
par_trajGen.IMU.gyro.noise.R = 0 * ones(3, 1) * cst.deg2Rad / (3600^2); % 转换前单位°/h^2，转换后单位rad/s^2
par_trajGen.IMU.gyro.noise.K = 0 * ones(3, 1) * cst.deg2Rad / (3600^(3/2)); % 转换前单位°/h^(3/2)，转换后单位rad/s^(3/2)
par_trajGen.IMU.gyro.noise.N = 0 * ones(3, 1) * cst.deg2Rad / 60; % 转换前单位°/sqrt(h)，转换后单位rad/sqrt(s)
par_trajGen.IMU.gyro.noise.Q = realmin * ones(3, 1) * cst.sec2Rad; % 转换前单位″，转换后单位rad
% 惯组杆臂
par_trajGen.IMU.lV = zeros(3,1);
% 位移计杆臂
par_trajGen.displm.lV = zeros(3,1);

%% 初始条件
iniCon_trajGen.CVG = angle2dcm_cg(45*pi/180,0,0,'ZXY');
iniCon_trajGen.CBV = eye(3); % 初始姿态矩阵
iniCon_trajGen.vG = [0; 0; 0]; % 初始速度
iniCon_trajGen.pos = [30*cst.deg2Rad, 110*cst.deg2Rad, 100]'; %#ok<*STRNU> % 纬度、经度、高度

cst_trajGen = structcpy_specfields(struct(), cst, {'omegaIE', 'RE', 'eSq', 'f'});
[out_trajGen, ~] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);

figure;plot(out_trajGen.t, out_trajGen.omegaNVV*180/pi);grid on;xlabel('t/(s)');ylabel('角速度/(°/s)');legend('X', 'Y', 'Z');
figure;plot(out_trajGen.t, out_trajGen.bodyAtt*180/pi);grid on;xlabel('t/(s)');ylabel('姿态角/(°)');legend('Z', 'Y', 'X');
figure;plot(out_trajGen.t, out_trajGen.pos(:,end,3));grid on;xlabel('t/(s)');ylabel('高度/(m)');
figure;plot(out_trajGen.t, out_trajGen.vG(:,:,3));grid on;xlabel('t/(s)');ylabel('速度/(m/s)');
figure;plot(out_trajGen.pos(:,2,3)/cst.deg2Rad,out_trajGen.pos(:,1,3)/cst.deg2Rad,'d','LineWidth',2);grid on;

%% 惯性导航参数配置
% sinistep_pgs函数配置
cfg_SINI.NSInLS = 1;
cfg_SINI.HSInNS = 1;
cfg_SINI.useDCMInAttCalc = true;
cfg_SINI.useNormOrthoInAttCalc = true;
cfg_SINI.useHiResPosNSCal = false;
cfg_SINI.useExtGravity = true;
cfg_SINI.navCoordType = NavCoordType.GEO;
%
par_SINI.Tl = par_trajGen.samplePeriod;
par_SINI.C = [0 0 0];
% sinistep_pgs函数初始条件
initCon_SINI.L_n = out_trajGen.pos(1, 1);
initCon_SINI.L_n1 = initCon_SINI.L_n;
initCon_SINI.lambda_n = out_trajGen.pos(1, 2);
initCon_SINI.lambda_n1 = initCon_SINI.lambda_n;
initCon_SINI.h_n = out_trajGen.pos(1, 3);
initCon_SINI.h_n1 = initCon_SINI.h_n;
initCon_SINI.vN_m = out_trajGen.vG(1, 1:3)' + 0*[ 1 2 3 ]';
initCon_SINI.vN_m1 = initCon_SINI.vN_m;
initCon_SINI.vN_n1 = initCon_SINI.vN_m;
initCon_SINI.CBL = [0 1 0; 1 0 0; 0 0 -1]*out_trajGen.CBG(:, :, 1);
% 常量
cst_SINI = load('earthconstants_WGS84.mat', 'RE', 'f', 'mu', 'J2', 'J3', 'omegaIE');
CNL = [0 1 0; 1 0 0; 0 0 -1];

%% 输出参数初始化
tmp_sampleNum = size(out_trajGen.IMU.accOutErrFree, 1);
saveSpan = 1;
tmp_saveNum = fix(tmp_sampleNum / saveSpan);
ref.pos = NaN(tmp_saveNum, 3);
ref.vG = NaN(tmp_saveNum, 3);
ref.CBG = NaN(3,3,tmp_saveNum);
saveCnt = 0;

%%
hwb = waitbar(0, 'Integrated Navigation Computation...');
for i = 0:tmp_sampleNum % =0时初始化（对应粗对准的结束时刻），>0时为解算周期
    if i==0
        mode = int8(0);
        accOut = out_trajGen.IMU.accOut(1, :)'; % 不需要加惯性器件误差时使用accOutErrFree，需要时使用accOut，下同
        gyroOut = out_trajGen.IMU.gyroOut(1, :)';
        initCon_SINI.specVelInc_lx = accOut; % 0时刻之前的比速度增量
        initCon_SINI.angleInc_lx = gyroOut;
    else
        mode = int8(1);
        accOut = out_trajGen.IMU.accOut(i, :)';
        gyroOut = out_trajGen.IMU.gyroOut(i, :)';
    end
    % waitbar
    if (mod(i, floor(tmp_sampleNum/100))==0)
        waitbar(double(i)/double(tmp_sampleNum), hwb);
    end
    %
    in_SINI = struct('specVelInc', accOut, 'angleInc', gyroOut, 'g', par_trajGen.g,  'hAltm', 0,...
        'CNEExt', NaN(3), 'hExt', NaN, 'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
        'beta_m_ext', NaN(3,1), 'DvScul_m_ext', NaN(3,1), 'DRScrl_m_ext', NaN(3,1), 'isReverseNav', false);
    [out_SINIStep, auxOut_SINIStep] = sinistep_pgs(mode, in_SINI, cfg_SINI, par_SINI, initCon_SINI, cst_SINI);
    % 初始化
    if i == 0
        out_SINI = repmat(out_SINIStep,tmp_saveNum,1);
        auxOut_SINI = repmat(auxOut_SINIStep,tmp_saveNum,1);
        continue;
    end
    %%
    if (mod(i,saveSpan) == 0 && i~= 0)
        saveCnt = saveCnt + 1;
        out_SINI(saveCnt) = out_SINIStep;
        auxOut_SINI(saveCnt) = auxOut_SINIStep;
        %
        ref.pos(saveCnt,:) = out_trajGen.pos(i+1,:,1);
        ref.vG(saveCnt,:) = out_trajGen.vG(i+1,:,1);
        ref.CBG(:,:,saveCnt) = out_trajGen.CBG(:,:,i+1);
    end
end
close(hwb);
%
out_SINI = structarray2structofarray(out_SINI);
auxOut_SINI = structarray2structofarray(auxOut_SINI);
out_SINI.t = out_SINI.cycCnt*par_SINI.Tl;
out_SINI.pos = [out_SINI.L, out_SINI.lambda, out_SINI.h];
ref.t = out_SINI.t;
%
comparepos({out_SINI.pos}, {out_SINI.t}, ref.pos, ref.t, 'RE', 6378137, 'hFig', [], 'cmpName', {'导航'}, 'refName', '参考值', 'figName', '位置比较', 'plotInOneFig', true);
comparevector({out_SINI.vN}, {out_SINI.t}, ref.vG, ref.t,'cmpName', {'导航'}, 'figName', '速度比较');
compareatt_dcm({out_SINI.CBG}, {out_SINI.t}, ref.CBG, ref.t, [], {'导航'}, [], '姿态比较', [], true);

%% 轨迹逆推
% 真实轨迹
% pos = out_trajGen.pos(2:end,:,1);
% vG = out_trajGen.vG(2:end,:,1);
% CBL = out_trajGen.CBL(:,:,2:end);
% 数值积分轨迹,结果相同
pos = out_SINI.pos;
vG = out_SINI.vN;
CBL = out_SINI.CBL;
%
cfg_back.CLLType = 1;
cfg_back.rotVecType = 2;

par_back.g = par.g;
par_back.dt = par.dt;
par_back.useExtGravity = par.useExtGravity;

cst_back.CNL = [0 1 0; 1 0 0; 0 0 -1];
cst_back.uZNN = [0 0 1]';
cst_back.RE = cst.RE;
cst_back.f = cst.f;
cst_back.mu = cst.mu;
cst_back.J2 = cst.J2;
cst_back.J3 = cst.J3;
cst_back.omegaIE = cst.omegaIE;

num = length(pos);
acce = NaN(num,3);
gyro = NaN(num,3);
%
tic
for i=0:num
    if i == 0
        in_back.CBL = CBL(:,:,1);
        in_back.vN = vG(1,:)';
        in_back.pos = pos(1,:)';
        [acceOut, gyroOut] = navpar2imuout_inc(int8(0), in_back,  cfg_back, par_back, cst_back);
    else
        in_back.CBL = CBL(:,:,i);
        in_back.vN = vG(i,:)';
        in_back.pos = pos(i,:)';
        [acceOut, gyroOut] = navpar2imuout_inc(int8(1), in_back,  cfg_back, par_back, cst_back);
        acce(i,:) = acceOut';
        gyro(i,:) = gyroOut';
    end
end
toc
%
figure;plot(out_trajGen.t(2:end),(out_trajGen.IMU.gyroOut-gyro)*180/pi*3600/par.dt,'-d');
grid on;xlabel('time / (s)');ylabel('°/h');title('陀螺输出比较');legend('x','y','z');
figure;plot(out_trajGen.t(2:end),(out_trajGen.IMU.accOut-acce)/par.dt*1.0e5,'-d');
grid on;xlabel('time / (s)');ylabel('μg');title('加表输出比较');legend('x','y','z');
